#include <stdio.h>
#include "platform.h"
#include "xil_printf.h"
#include "xeuclidean_distance.h"
#include "xtime_l.h"
#include "xil_cache.h"

#define DIM 128

#define COUNTS_PER_USECOND  (XPAR_CPU_CORTEXA9_CORE_CLOCK_FREQ_HZ / (2U*1000000U))

// Time Measure
u64 getDlyUs(void)
{
    XTime dly;
    static XTime Xtime_last = 0;
    XTime Xtime_curr;

    XTime_GetTime(&Xtime_curr);
    dly = Xtime_curr - Xtime_last;
    Xtime_last = Xtime_curr;
    return dly / COUNTS_PER_USECOND;
}

void L2acc_Init(XEuclidean_distance *l2_acc)
{
    int status = XEuclidean_distance_Initialize(l2_acc, 0);
    if (status != XST_SUCCESS) {
        printf("XEuclidean_distance_Initialize failed, status: %d\n", status);
    } else {
        printf("XEuclidean_distance_Initialize success\n");
    }
}

float L2Distance(float* list1, float* list2, int dim, XEuclidean_distance *l2_acc)
{
    float result;
    XEuclidean_distance_Set_a(l2_acc, (u32)(uintptr_t)list1);
    XEuclidean_distance_Set_b(l2_acc, (u32)(uintptr_t)list2);
    XEuclidean_distance_Set_result(l2_acc, (u32)(uintptr_t)&result);

    Xil_DCacheFlushRange((INTPTR)list1, sizeof(float) * dim);
    Xil_DCacheFlushRange((INTPTR)list2, sizeof(float) * dim);
    Xil_DCacheFlushRange((INTPTR)&result, sizeof(float));

    XEuclidean_distance_Start(l2_acc);

    while (!XEuclidean_distance_IsDone(l2_acc));

    Xil_DCacheInvalidateRange((INTPTR)&result, sizeof(float));

    return result;
}

float L2Distance_soft(float* list1, float* list2, int dim)
{
    float soft_result = 0.0;
    for (int i = 0; i < dim; i++) {
        float diff = list1[i] - list2[i];
        float diff2 = diff * diff;
        soft_result += diff2;
    }
    return soft_result;
}

void fill_array(float* array, int length, int seed)
{
    for (int i = 0; i < length; i++) {
        seed = (((seed + 723) * (seed + 430)) >> (seed % 7)) % 1000;
        array[i] = seed / 1000.0;
    }
}

int main()
{
    init_platform();

    printf("Hello World!!!\n\r");
    printf("Init PE\n\r");
    XEuclidean_distance l2_acc;
    L2acc_Init(&l2_acc);
    printf("Init PE Done\n\r");

    float list1[DIM];
    float list2[DIM];

    fill_array(list1, DIM, 23);
    fill_array(list2, DIM, 123);

    printf("list1: ");
    for (int i = 0; i < DIM; i++) {
        printf("%.3f,", list1[i]);
    }
    printf("\r\n");

    printf("list2: ");
    for (int i = 0; i < DIM; i++) {
        printf("%.3f,", list2[i]);
    }
    printf("\r\n");

    printf("Start Cal L2 by hardware\r\n");
    getDlyUs();
    float result = L2Distance(list1, list2, DIM, &l2_acc);
    unsigned long delay = getDlyUs();
    printf("Hard Result: %f, spend time: %lu us\r\n", result, delay);

    printf("Start Cal L2 by software\r\n");
    getDlyUs();
    float result_soft = L2Distance_soft(list1, list2, DIM);
    delay = getDlyUs();
    printf("Soft Result: %f, spend time: %lu us\r\n", result_soft, delay);

    cleanup_platform();
    return 0;
}
